clc;
clear;
% 2021.10.09
% 使用ss函数进行状态空间构建
% 参数：1200kg、(200, 200, 150)kN/m、(0.02, 0.02, 0.04)N·s/m
mass_m = 1200 * eye(3);
stif_m = 1000*[400 -200 0;
    -200 350 -150;
    0 -150 150];
% mass_m\stif_m
cri_damp_m = [0.02, 0.02, 0.04];
% omega
omega_m = diag(sqrt(stif_m ./ mass_m));    % 0.4082    0.4082    0.3536

i=1;
j=3;

alpha = 2*omega_m(i)*omega_m(j)/(omega_m(j)^2-omega_m(i)^2)*(omega_m(j)*cri_damp_m(i)-omega_m(i)*cri_damp_m(j));
beta = 2*omega_m(i)*omega_m(j)/(omega_m(j)^2-omega_m(i)^2)*(-cri_damp_m(i)/omega_m(j)+cri_damp_m(j)/omega_m(i));

damp_m = alpha * mass_m + beta * stif_m;    % 19.5959   19.5959   33.9411


% state matrix
A  = [zeros(3) eye(3);
    -stif_m/mass_m -damp_m/mass_m];
% input matrix
B = [zeros(1,3) 1 1 1]'/1200;
% output matrix [x1' x2' x3' x1" x2" x3"]
C = [0 0 0 0 0 1];
% throughtput matrix ?
D = 0;
% ss_structural
state_ss = ss(A, B, C, D);

trans_ss = tf(state_ss);

% conmput
t = 1:0.01:10;
% P如何作用在系统上
P = 10*sin(10*t);
% P = 10 * heaviside(t-2);

out_S = lsim(state_ss, P, t);
out_T = lsim(trans_ss, P, t);

% state space 与 tranform function 计算结果一致
subplot(2, 2, 1);
plot(t, P, t, out_S)
subplot(2, 2, 2);
plot(t, P, t, out_T)
subplot(2, 2, 3);
bode(state_ss)
subplot(2, 2, 4);
bode(trans_ss)
